import sys
import time
import matplotlib.pyplot as plt
import math
import numpy

from PointInPolygons import multi_polygon

src = 0  # 起始节点
dest = 1  # 目标节点
dis = []
dis.append(src)


class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y

    def is_equal(self, node):
        if isinstance(node, self.__class__):
            if node.x == self.x and node.y == self.y:
                return True
        return False


class Edge:
    def __init__(self, node1: Node, node2: Node):
        self.node1 = node1
        self.node2 = node2
        self.length = math.sqrt(math.pow(node1.x - node2.x, 2) + math.pow(node1.y - node2.y, 2))

    def is_intersect(self, edge):
        """
                判断该线段是否和目标线段相交
                这里假设自己是AB，目标线段是CD
                :param edge:
                :return:
                """
        if isinstance(edge, self.__class__):
            A = numpy.array([self.node1.x, self.node1.y])
            B = numpy.array([self.node2.x, self.node2.y])
            C = numpy.array([edge.node1.x, edge.node1.y])
            D = numpy.array([edge.node2.x, edge.node2.y])
            return (numpy.dot(numpy.cross((C - A), (D - A)), numpy.cross((C - B), (D - B))) < 0 and
                    numpy.dot(numpy.cross((A - C), (B - C)), numpy.cross((A - D), (B - D))) < 0
                    )

    def is_equal(self, edge):
        if isinstance(edge, self.__class__):
            if edge.node1.is_equal(self.node1) and edge.node2.is_equal(self.node2):
                return True
            if edge.node1.is_equal(self.node2) and edge.node2.is_equal(self.node1):
                return True
        return False


class Polygon:
    def __init__(self, edges):
        self.edges = edges
        self.nodes = []
        for edge in self.edges:
            if edge.node1 not in self.nodes:
                self.nodes.append(edge.node1)
            if edge.node2 not in self.nodes:
                self.nodes.append(edge.node2)

    def are_adjacent(self, node1, node2):
        for edge in self.edges:
            if (edge.node1 == node1 and edge.node2 == node2) or (edge.node1 == node2 and edge.node2 == node1):
                return True
        return False


class Graph:
    def __init__(self, vertices):
        self.V = vertices
        self.graph = [[0 for column in range(vertices)] for row in range(vertices)]

    def min_distance(self, dist, spt_set):
        min_dist = sys.maxsize
        min_index = 0

        for v in range(self.V):
            if dist[v] < min_dist and spt_set[v] == False:
                min_dist = dist[v]
                min_index = v

        return min_index

    def dijkstra(self, src, dest):
        # 初始化各数组
        dist = [sys.maxsize] * self.V
        dist[src] = 0
        spt_set = [False] * self.V
        parent = [-1] * self.V

        for cout in range(self.V):
            u = self.min_distance(dist, spt_set)
            spt_set[u] = True

            for v in range(self.V):
                if self.graph[u][v] > 0 and spt_set[v] == False and dist[v] > dist[u] + self.graph[u][v]:
                    dist[v] = dist[u] + self.graph[u][v]
                    parent[v] = u

        self.print_solution(src, dest, dist, parent)

    def print_solution(self, src, dest, dist, parent):
        print("路径")
        print(f"{src}", end=" ")

        self.print_path(dest, parent)

        print(f"\n最短距离为: {dist[dest]}")

    def print_path(self, j, parent):
        if parent[j] == -1:
            return
        self.print_path(parent[j], parent)
        print(f"-> {j}", end=" ")
        dis.append(j)


Start = Node(160, 260)

Goal = Node(436, 21)

nodes = [Start, Goal, Node(44, 260), Node(211, 260), Node(211, 195), Node(44, 195),  # 横矩形
         Node(88, 10), Node(27, 78), Node(37, 146), Node(94, 163), Node(128, 75),  # 五边形
         Node(127, 169), Node(150, 64), Node(175, 169),  # 上三角
         Node(178, 98), Node(178, 16), Node(219, 10), Node(248, 48),  # 左四边形
         Node(219, 135), Node(235, 229), Node(270, 187),  # 下三角
         Node(327, 154), Node(257, 154), Node(258, 17), Node(327, 17),  # 纵矩形
         Node(371, 189), Node(336, 35), Node(361, 15), Node(382, 40),  # 右四边形
         Node(327, 265), Node(296, 240), Node(296, 192), Node(335, 161), Node(363, 192), Node(363, 243), ]  # 六边形
edges = [Edge(Node(44, 260), Node(211, 260)), Edge(Node(211, 260), Node(211, 195)), Edge(Node(211, 195), Node(44, 195)),
         Edge(Node(44, 260), Node(44, 195)),  # 横矩形
         Edge(Node(88, 10), Node(27, 78)), Edge(Node(27, 78), Node(37, 146)), Edge(Node(37, 146), Node(94, 163)),
         Edge(Node(94, 163), Node(128, 75)), Edge(Node(88, 10), Node(128, 75)),  # 五边形
         Edge(Node(127, 169), Node(150, 64)), Edge(Node(150, 64), Node(175, 169)), Edge(Node(127, 169), Node(175, 169)),
         # 上三角
         Edge(Node(178, 98), Node(178, 16)), Edge(Node(178, 16), Node(219, 10)), Edge(Node(219, 10), Node(248, 48)),
         Edge(Node(178, 98), Node(248, 48)),  # 左四边形
         Edge(Node(219, 135), Node(235, 229)), Edge(Node(235, 229), Node(270, 187)),
         Edge(Node(219, 135), Node(270, 187)),  # 下三角
         Edge(Node(327, 154), Node(257, 154)), Edge(Node(257, 154), Node(258, 17)), Edge(Node(258, 17), Node(327, 17)),
         Edge(Node(327, 154), Node(327, 17)),  # 纵矩形
         Edge(Node(371, 189), Node(336, 35)), Edge(Node(336, 35), Node(361, 15)), Edge(Node(361, 15), Node(382, 40)),
         Edge(Node(371, 189), Node(382, 40)),  # 右四边形
         Edge(Node(327, 265), Node(296, 240)), Edge(Node(296, 240), Node(296, 192)),
         Edge(Node(296, 192), Node(335, 161)),
         Edge(Node(335, 161), Node(363, 192)), Edge(Node(363, 192), Node(363, 243)),
         Edge(Node(327, 265), Node(363, 243))]  # 六边形

polygons = [Polygon(
    [Edge(Node(44, 260), Node(211, 260)), Edge(Node(211, 260), Node(211, 195)), Edge(Node(211, 195), Node(44, 195)),
     Edge(Node(44, 260), Node(44, 195))]),  # 横矩形
    Polygon([Edge(Node(88, 10), Node(27, 78)), Edge(Node(27, 78), Node(37, 146)),
             Edge(Node(37, 146), Node(94, 163)),
             Edge(Node(94, 163), Node(128, 75)), Edge(Node(88, 10), Node(128, 75))]),  # 五边形
    Polygon([Edge(Node(127, 128), Node(150, 64)), Edge(Node(150, 64), Node(175, 169)),
             Edge(Node(127, 128), Node(175, 169))]),  # 上三角
    Polygon([Edge(Node(178, 98), Node(178, 16)), Edge(Node(178, 16), Node(219, 10)),
             Edge(Node(219, 10), Node(248, 48)),
             Edge(Node(178, 98), Node(248, 48))]),  # 左四边形
    Polygon([Edge(Node(219, 135), Node(235, 229)), Edge(Node(235, 229), Node(270, 187))
                , Edge(Node(219, 135), Node(270, 187)), ]),  # 下三角
    Polygon([Edge(Node(327, 154), Node(257, 154)), Edge(Node(257, 154), Node(258, 17)),
             Edge(Node(258, 17), Node(327, 17)),
             Edge(Node(327, 154), Node(327, 17)), ]),  # 纵矩形
    Polygon([Edge(Node(371, 189), Node(336, 35)), Edge(Node(336, 35), Node(361, 15)),
             Edge(Node(361, 15), Node(382, 40)),
             Edge(Node(371, 189), Node(382, 40)), ]),  # 右四边形
    Polygon([Edge(Node(327, 265), Node(296, 240)), Edge(Node(296, 240), Node(296, 192)),
             Edge(Node(296, 192), Node(335, 161)),
             Edge(Node(335, 161), Node(363, 192)), Edge(Node(363, 192), Node(363, 243)),
             Edge(Node(327, 265), Node(363, 243))])  # 六边形
]

length = len(nodes)
# matrix = [[10000 for _ in range(length)] for _ in range(length)]
matrix = [[0 for _ in range(length)] for _ in range(length)]
for i in range(length):
    matrix[i][i] = 0
for i in range(length):
    for j in range(i + 1, length):
        x = 0  # 判断两点之间的线段是否和其他线段相交，默认不相交
        y = 0  # 判断nodes[i]是否在该多边形中
        z = 0  # 判断nodes[j]是否在该多边形中
        for polygon in polygons:
            # 这一步判断这两个点是否属于同一个多边形
            for node in polygon.nodes:
                if nodes[i].is_equal(node):
                    y = 1
                if nodes[j].is_equal(node):
                    z = 1
            if y == 1 and z == 1:
                # 记录该多边形，之后用
                polygon_mark = polygon
                break
            else:
                y = 0
                z = 0
        if y == 1 and z == 1:
            # 如果是在相同多边形进一步判断是否属于邻点,默认不是邻点
            for edge in polygon_mark.edges:
                if Edge(nodes[i], nodes[j]).is_equal(edge) or Edge(nodes[j], nodes[i]).is_equal(edge):
                    x = 1
                    matrix[i][j] = Edge(nodes[i], nodes[j]).length
                    matrix[j][i] = Edge(nodes[i], nodes[j]).length
        else:
            for edge in edges:
                if edge.is_intersect(Edge(nodes[i], nodes[j])):
                    x = 1
            if x == 0:
                matrix[i][j] = Edge(nodes[i], nodes[j]).length
                matrix[j][i] = Edge(nodes[i], nodes[j]).length

g = Graph(length)
g.graph = matrix

# 开始时间
start_time = time.perf_counter()

pointIn = (Start.x, Start.y)
pointInPolygons = multi_polygon.is_inside(pointIn)
if pointInPolygons:
    print(f"是否在多边形的内部{multi_polygon.is_inside(pointIn)},没有正确路径")
else:
    g.dijkstra(src, dest)

# 结束时间
end_time = time.perf_counter()
run_time = end_time - start_time

print("程序运行时间：", run_time, "秒")
print("经过的节点数量", len(dis))
i = 2

for node in nodes:
    plt.scatter(node.x, node.y, color='blue')  # 蓝色表示节点
    if i > 0 and i == 2:
        plt.text(node.x, node.y, 'Start', fontsize=10, verticalalignment='bottom', horizontalalignment='right')
        i -= 1
    elif i > 0 and i == 1:
        plt.text(node.x, node.y, 'Goal', fontsize=10, verticalalignment='bottom', horizontalalignment='right')
        i -= 1

for edge in edges:
    plt.plot([edge.node1.x, edge.node2.x], [edge.node1.y, edge.node2.y], color="red")  # 红色表示障碍物

for i in range(len(dis)):
    for j in range(i + 1, len(dis)):

        if j == i + 1:
            plt.plot([nodes[dis[i]].x, nodes[dis[j]].x], [nodes[dis[i]].y, nodes[dis[j]].y], color="black")  # 黑色表示路线
plt.show()
